#include "ICSP.h"

DigitalPin<SCK> sckPin;
DigitalPin<MISO> misoPin;
DigitalPin<MOSI> mosiPin;
DigitalPin<RESET> resetPin;

unsigned long ICSPCommand (uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
	uint8_t n, m, o;
	SPI.transfer(a);
	n=SPI.transfer(b);
//	if (n != a) Serial.println("spi Error a");
	m=SPI.transfer(c);
//	if (m != b) Serial.println("spi Error b");
	o=SPI.transfer(d);
//	if (o != c) Serial.println("spi Error c");

	return 0xFFFFFF & ((n<<16)+(m<<8) + o);
}

void programModeCommand(){
	// special command to ignore errors when initing for the first time
	uint8_t ret;
	SPI.transfer(ICSP_EXTENDEDCOMMAND);
	SPI.transfer(ICSP_PROGENABLE);
	ret = SPI.transfer(0x00);
//	if (ret != 0x53) Serial.println("spi Error C");
	ret = SPI.transfer(0x00);
//	if (ret != 0x00) Serial.println("spi Error D");
}

void startProgramMode () {
	// datasheet says hold SCK low, pulse RESET high for at least 2 cycles, then hold RESET low.
	//wait at least 20 MS before giving programming enable code

	SPI.begin ();
	SPI.setClockDivider(SPI_CLOCK_DIV4);
//	Serial.println("...spi_init done");
//	resetPin.outputMode();
	pinMode(RESET,OUTPUT);
	resetPin.low();
//	sckPin.outputMode();
	pinMode(SCK,OUTPUT);
	sckPin.low();
	resetPin.high();
	asm volatile ("nop\nnop\nnop\nnop"); // wait for at least 2 cycles while SCK has been held low
	resetPin.low();
	delay(21);
//	misoPin.inputMode();
	pinMode(MISO,OUTPUT);
//	mosiPin.outputMode();
	pinMode(MOSI,OUTPUT);
	programModeCommand();
}

void endProgramMode () {
	SPI.end ();
	misoPin.low();
	mosiPin.low();
	sckPin.low();/*
	misoPin.inputMode();
	mosiPin.inputMode();
	sckPin.inputMode();*/
	pinMode(MISO,INPUT);
	pinMode(MOSI,INPUT);
	pinMode(SCK,INPUT);
}

uint16_t readSignature () {
	uint8_t MSB, LSB;
	MSB = ICSPCommand(ICSP_READSIGNATURE, 0x00, 0x01, 0x00);
	LSB = ICSPCommand(ICSP_READSIGNATURE, 0x00, 0x02, 0x00);
	return ((MSB << 8) + LSB);
}

void programFuse (uint8_t fuse, uint8_t value){

	switch (fuse) {
		case FUSE_PROT:
			ICSPCommand(ICSP_EXTENDEDCOMMAND, ICSP_WRITELOCKBITS, 0x00, value);
			break;
		case FUSE_LOW:
			ICSPCommand(ICSP_EXTENDEDCOMMAND, ICSP_WRITEFUSEBITSLOW, 0x00, value);
			break;
		case FUSE_HIGH:
			ICSPCommand(ICSP_EXTENDEDCOMMAND, ICSP_WRITEFUSEBITSHIGH, 0x00, value);
			break;
		case FUSE_EXT:
			ICSPCommand(ICSP_EXTENDEDCOMMAND, ICSP_WRITEFUSEBITSEXT, 0x00, value);
			break;
		default:
			break;
	}
	while(ICSPCommand(ICSP_RDYBSY, 0, 0, 0) & 1){};//wait for RDY/BSY to drop
}

void chipErase(){
	// datasheet requires 9.0ms wait time on erase minimum
	ICSPCommand(ICSP_EXTENDEDCOMMAND, ICSP_CHIPERASE, 0, 0);

	while(ICSPCommand(ICSP_RDYBSY, 0, 0, 0) & 1){};//wait for RDY/BSY to drop

}

uint16_t pageAddress (uint16_t address, uint8_t targetPageSize) {
	switch (targetPageSize) {
		case 32:
			return address & 0xFFFFFFF0;
			break;
		case 64:
			return address & 0xFFFFFFE0;
			break;
		case 128:
			return address & 0xFFFFFFC0;
			break;
		default:
			return address;
			break;
	}
}

void programPage (uint8_t pageBuffer[],const uint16_t bufferSize, uint16_t startAddress, const uint8_t targetPageSize){
  uint16_t currentWord;
  uint16_t currentPage;

  currentWord = startAddress>>1;		//get the word address of the current byte
  currentPage = pageAddress(currentWord,targetPageSize);  //get the page address of the current word

  while ((currentWord<<1)-startAddress < (bufferSize)) { // as long as the current word is inside of the buffer size
    if (currentPage != pageAddress(currentWord,targetPageSize)) { // if the page address has changed
    	commitPage(currentPage);  //commit the current page
    	currentPage = pageAddress(currentWord,targetPageSize); // and start a new one
    }
	ICSPCommand(ICSP_LOADPROGMEMLOW, (currentWord >> 8) & 0xFF, currentWord & 0xFF, pageBuffer[(currentWord<<1)-startAddress]); // set low byte
	ICSPCommand(ICSP_LOADPROGMEMHIGH, (currentWord >> 8) & 0xFF, currentWord & 0xFF, pageBuffer[(currentWord<<1)-startAddress+1]); // set high byte

    currentWord++; //increment current word position
  }

  commitPage(currentPage); // make sure the final page is committed
}

void commitPage (uint16_t address) {
	//datasheet requires 4.5ms minimum delay on flash write
	ICSPCommand(ICSP_COMMITPROGMEM, (address >> 8) & 0xFF, address & 0xFF, 0);
	while(ICSPCommand(ICSP_RDYBSY, 0, 0, 0) & 1){};//wait for RDY/BSY to drop
}
